/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package dungeonsolver.controller.algorithm;

import dungeonsolver.helper.Utility;
import dungeonsolver.model.Labyrinth;
import java.awt.Point;
import java.util.Vector;

/**
 *
 * @author LegACy
 */
public class AlgorithmAStar implements IAlgorithm {

	protected class Node {
		public Node(Node parent, Point position, int heuristic) {
			//Set variable
			m_Parent	= parent;
			m_Position	= position;

			//Calculate
			m_Cost		= m_Parent == null ? 0 : (parent.getCost() + 1);
			m_TotalCost	= m_Cost + heuristic;
		}

		public Node getParent()		{	return m_Parent;	};
		public Point getPosition()	{	return m_Position;	};
		public int getCost()		{	return m_Cost;		};
		public int getTotalCost()	{	return m_TotalCost;	};

		protected Node	m_Parent;
		protected Point m_Position;
		protected int	m_Cost;
		protected int	m_TotalCost;
	}

	public Point[] getPath(Labyrinth labyrinth, Point from, Point to) {
		//Check
		if (labyrinth == null || from.equals(to)) return new Point[0];

		//Create visited map
		boolean[][] Map			= labyrinth.getVisitMap();
		boolean[][] Activated	= labyrinth.getVisitMap();

		//Create variables
		Node Current			= new Node(null, from, 0);
		Vector<Node> Nodes		= new Vector<Node>();
		Vector<Node> AllNodes	= new Vector<Node>();
		Nodes.add(Current);

		Utility.instance().addLog("Add [" + Current.getPosition().x + "," + Current.getPosition().y + "] with cost: " + Current.getTotalCost());
		
		//While not destination
		while (Current != null && (Current.getPosition().x != to.x || Current.getPosition().y != to.y)) {
			//Set current position as visited
			AllNodes.add(Current);
			Map[Current.getPosition().x][Current.getPosition().y]		= true;
			Activated[Current.getPosition().x][Current.getPosition().y] = true;

			//For each neighbor
			Point[] Neighbors = labyrinth.getNeighbors(Current.getPosition());
			for (int i = 0; i < Neighbors.length; i++) {
				//If not visited
				if (!Map[Neighbors[i].x][Neighbors[i].y] && !Activated[Neighbors[i].x][Neighbors[i].y]) {
					//Create and add node
					Node NewNode = new Node(Current,
											Neighbors[i],
											Utility.instance().calculateManhattanRange(Neighbors[i], to));
					Nodes.add(NewNode);

					//Set as activated
					Activated[Neighbors[i].x][Neighbors[i].y] = true;

					//Log
					Utility.instance().addLog("Add [" + NewNode.getPosition().x + "," + NewNode.getPosition().y + "] with cost: " + NewNode.getTotalCost());
				}
			}

			//For each active node
			Nodes.remove(Current);
			Node Next = Nodes.isEmpty() ? null : Nodes.firstElement();
			for (int i = 0; i < Nodes.size(); i++) {
				//Find the one with the lowest cost
				if (Nodes.elementAt(i).getTotalCost() < Next.getTotalCost()) Next = Nodes.elementAt(i);
			}

			//Log
			Utility.instance().addLog("Next: [" + Next.getPosition().x + "," + Next.getPosition().y + "] with the lowest cost: " + Next.getTotalCost());

			//Set as current
			Current = Next;
		}

		//While cur
		Vector<Point> Path = new Vector<Point>();
		while (Current != null) {
			//Add
			if (Current.getParent() != null) Path.add(0, Current.getPosition());
			Current = Current.getParent();
		}

		//Create array
		Point[] PathArray = new Point[Path.size()];
		for (int i = 0; i < PathArray.length; i++) PathArray[i] = Path.elementAt(i);

		//Return path
		return PathArray;
	}

}
